import random
import math
import pygame
import sys
from pprint import pprint
from math import pi
from math import sqrt
from astree import Point
from astree import ASTree
from readLevel import ReadLevel

level = None

class AgentBrain():
    def __init__(self):
        global level
        """Put any initialization code here."""
        level = ReadLevel('..\levels\\clover2.py')
        
        self.paths = level.tree
        self.walls = level.walls
        
        self.expQueue = []
      
    def action(self, observation):
        action = {
            'turn': random.random()*pi/1.5 - pi/3,
            'speed': random.random()*50,
            'shoot': random.random() > 0.9 #shoot 10% of the turns
        }
        return action

    def theta_star(self, start, goal):
        global level
        self.expQueue = []
        closed = [] 
        s_start = level.tree.addSplit(start[0], start[1], True)
        s_start.g = 0
        s_start.parent = s_start
        s_goal = level.tree.addSplit(goal[0], goal[1], True)

        s_start.val = cost(s_start, s_goal)        
        self.expQueue.append(s_start)
        while len(self.expQueue):
            s = self.expQueue.pop(0)
            if s == s_goal:
                return s
            closed.append(s)
            for k in s.neigh:
                if not s.neigh[k] is None: #TEMP fix attempt (if + indent 9 lines) (nout)
                    s_1 = s.neigh[k]
                    if not line_of_sight(s.loc(), s_1.loc()):
                        continue
                    if not (s_1 in closed):
                        if not (s_1 in self.expQueue):
                            s_1.g = 100000
                            s_1.parent = None
                        self.update_point(s, s_1, s_goal)
        return None

    def update_point(self, s, s_1, s_goal):
        g_old = s_1.g
        compute_cost(s, s_1)
        if s_1.g < g_old:
            if s_1 in self.expQueue:
                self.expQueue.remove(s_1)
            s_1.val = s_1.g + cost(s_1, s_goal)
            self.expQueue.append(s_1)
            self.expQueue.sort()


#Theta star
def compute_cost(s, s_1):
    if line_of_sight(s.parent.loc(), s_1.loc()):
        if s.parent.g + cost(s.parent, s_1) < s_1.g:
            s_1.parent = s.parent
            s_1.g = s.parent.g + cost(s.parent, s_1)
    else:
        if s.g + cost(s, s_1) < s_1.g:
            s_1.parent = s
            s_1.g = s.g + cost(s, s_1)

def cost(s, s_1):
    return sqrt((s_1.x - s.x)*(s_1.x - s.x)+(s_1.y - s.y)*(s_1.y - s.y))
        
def line_of_sight(s, s_1):
    for wall in level.walls:
        w = wall[0]
        w_1 = wall[1]
        if ((w_1[1]-s[1])*(w[0]-s[0]) > (w[1]-s[1])*(w_1[0]-s[0])) != ((w_1[1]-s_1[1])*(w[0]-s_1[0]) > (w[1]-s_1[1])*(w_1[0]-s_1[0])) \
            and ((w[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w[0]-s[0])) != ((w_1[1]-s[1])*(s_1[0]-s[0]) > (s_1[1]-s[1])*(w_1[0]-s[0])):
            return False
    return True

def test():
    agent = AgentBrain()
    path = agent.theta_star((673, 383), (427, 452))
    nextpath = path.parent
        
    pygame.init()
    window = pygame.display.set_mode((800, 800))
    level.displayUpdate(agent.paths, window)

    while(path != nextpath):
        pygame.draw.line(window, (102, 194, 33), path.loc(), nextpath.loc())
        path = nextpath
        nextpath = path.parent
    pygame.display.update()

    for w in agent.walls:
        pygame.draw.line(window, (255, 0, 0), w[0], w[1])
    pygame.display.update()
    first = True
        
    while True:
       for event in pygame.event.get(): 
            if event.type == pygame.QUIT: 
                sys.exit(0) 
            else: 
                if event.type == 6:
                    if first == True:
                        loc1 = event.pos
                        first = False
                    else:
                        loc2 = event.pos
                        path = agent.theta_star(loc1, loc2)
                        nextpath = path.parent

                        first = True
                        while(path != nextpath):
                            pygame.draw.line(window, (102, 194, 33), path.loc(), nextpath.loc())
                            print path.loc()
                            path = nextpath
                            nextpath = path.parent
                        pygame.display.update()
					
test()